import sympy
import math
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import csv
import os
from decimal import *
pre_yaw_deg = 0
pre_yaw_rad = pre_yaw_deg/180.0*math.pi
print(os.path.dirname(os.path.realpath(__file__)))
path = os.path.dirname(os.path.realpath(__file__))+'/../data/'
f = open(path+'line.csv', 'w')
dlc_writer = csv.writer(f)
dlc_writer.writerow(["x", "y", "theta", "kappa", "v", "s", "a"])

x = sympy.symbols('x')
z1 = 2.4/25.0*(x-10)-1.2
z2 = 2.4/25.0*(x-25)-1.2
dx1 = 25
dx2 = 21.95
dy1 = 5.0
dy2 = 5.0
y = dy1/2*(1+sympy.tanh(z1))-dy2/2*(1+sympy.tanh(z2))
# yaw = sympy.atan(dy1*sympy.cosh(z1)**(-2)*(1.2/dx1) -
#                 dy2*sympy.cosh(z2)**(-2)*(1.2/dx2))
yaw_calc = sympy.atan(sympy.diff(y, x))
der_1st = sympy.diff(y, x)
der_2nd = sympy.diff(der_1st, x)
kappa = der_2nd/((1+der_1st**2)**(1.5))
ds = (1+der_1st**2)**(1/2)
y_list = []
yaw_list = []
yaw_calc_list = []
kappa_list = []
s_list = []


start_length = 500
ex_x_np = np.arange(0, start_length, 0.25)
for ex_x in ex_x_np:
    temp_x = ex_x
    temp_y = 0
    temp_yaw = 0
    temp_x1 = temp_x*math.cos(pre_yaw_rad)-temp_y*math.sin(pre_yaw_rad)
    temp_y1 = temp_x*math.sin(pre_yaw_rad)+temp_y*math.cos(pre_yaw_rad)
    temp_yaw1 = temp_yaw+pre_yaw_deg
    dlc_writer.writerow(
        [str(Decimal.from_float(temp_x1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_y1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(temp_yaw1).quantize(Decimal('0.000000'))),
         str(Decimal.from_float(0).quantize(Decimal('0.000000'))),
         10.0,
         str(Decimal.from_float(ex_x).quantize(Decimal('0.000000'))),
         0.0])
